#include "mission_3_fsm.h"
#include "math.h"
#include "timeout.h"
#include "pid_app.h"
#include "user_led.h"
#include "user_beep.h"
#include "./OLED/OLED.h"
#include "./imu/imu_math.h"
#include "./imu/imu.h"
#include "./lw_pid/lw_pid.h"
#include "./lw_timer/lw_timer_callback.h"

/********************全局变量定义********************/
lwq_t mission_3_fsm_que; /*任务3状态机的消息队列*/
state_machine_t mission_3_fsm; /*任务3状态机*/

/********************静态变量定义********************/
static uint8_t line_inspection; /*巡黑线次第*/
static uint8_t walk_straight; /*走直线次第*/
static float marking_angle;
static float straight_marking_angle; /*标记的走直线偏航角度*/
static float line_marking_angle; /*标记的巡线预转向角度*/
static uint8_t in_place_reach_num; /*方向调整到位达标次数*/
static imu_euler car_euler_angle;
static uint16_t adjust_counter; /*裁决计数器*/

/********************静态函数声明********************/
static void mission_3_idle_handle(state_machine_t* me, param_event_t* e);
static void mission_3_ready_handle(state_machine_t* me, param_event_t* e);
static void walk_straight_line_handle(state_machine_t* me, param_event_t* e);
static void line_inspection_handle(state_machine_t* me, param_event_t* e);
static void direction_adjust_handle(state_machine_t* me, param_event_t* e);
static void line_inspec_preturn(state_machine_t* me, param_event_t* e);
static void gray_scale_white_handler(state_machine_t* me, param_event_t* e);

/********************全局函数定义********************/

/**
 * @brief	任务3状态机初始化
 */
void mission_3_fsm_init(void)
{
	lwq_create(&mission_3_fsm_que, QUE_MISSION_3_MSG_LEN, sizeof(param_event_t)); /*动态创建任务3消息队列*/
	mission_3_fsm.state = MISSION_3_IDLE; /*设置初始状态*/
	adjust_counter = 0;
}

/**
 * @brief	任务3状态机调度函数
 * @param	me 状态机变量
 * @param	event 事件变量
 */
void mission_3_fsm_dispatch(state_machine_t* me, param_event_t* e)
{
	switch(me->state)
	{
		case MISSION_3_IDLE:
			mission_3_idle_handle(me, e);
			break;
		
		case MISSION_3_READY:
			mission_3_ready_handle(me, e);
			break;
		
		case M3_WALK_STRAIGHT_LINE:
			walk_straight_line_handle(me, e);
			break;
		
		case M3_LINE_INSPECTION:
			line_inspection_handle(me, e);
			break;
		
		case M3_DIRECTION_ADJUST:
			direction_adjust_handle(me, e);
			break;
		
		case M3_LINE_INSPEC_PRETURN:
			line_inspec_preturn(me, e);
			break;
	}
}

/********************静态函数定义********************/

/*空闲状态事件处理*/
static void mission_3_idle_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case MISSION_3_ENTER:
		{			
			timeout_event_load(&mission_3_fsm_que, 800, STARTING_DELAY_TIMEOUT); /*挂载超时事件*/
			me->state = MISSION_3_READY; /*切换至就绪状态*/
			adjust_counter = 0;
			break;
		}
		
		case MISSION_3_EXIT:
			break;
	}
}

/*就绪状态事件处理*/
static void mission_3_ready_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case STARTING_DELAY_TIMEOUT:
		{
			line_inspection = 1; /*初始第一次巡黑线*/
			walk_straight = 1; /*初始第一次走直线*/
			imu_get_data(&car_euler_angle, false); /*读取欧拉角*/
			marking_angle = car_euler_angle.yaw; /*获取当前偏航角*/
			pid_set_target(&angle_pid, marking_angle); /*设置角度环目标*/
			pid_output_offset_disable(&angle_pid); /*取消角度环的输出偏移*/
			pid_set_target(&r_speed_pid, 50.0f); /*设置目标速度*/
			pid_set_target(&l_speed_pid, 50.0f);
			pid_enable(&angle_pid); /*使能角度环*/
			gray_twostate_scan_tim_start(&mission_3_fsm_que); /*开启双态灰度扫描*/
			me->state = M3_WALK_STRAIGHT_LINE; /*切换至走直线状态*/
			break;
		}			
	}
}

/*走直线状态事件处理*/
static void walk_straight_line_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case GRAY_SCALE_BLACK:
		{	
			adjust_counter++;
			
			if(adjust_counter == M3_ADJUST_THRE)
			{			
				led_set();
				beep_set();
				sound_light_tim_start(); /*执行声光提示*/
				gray_scan_tim_stop(); /*关闭灰度扫描*/
				pid_output_offset_enable(&angle_pid); /*启用角度环的输出偏移*/
				pid_set_target(&r_speed_pid, 0.0f); /*设置目标速度*/
				pid_set_target(&l_speed_pid, 0.0f);
				
				if(IS_ODD(walk_straight)) /*如果是奇数次走直线*/
				{						
					line_marking_angle = marking_angle + 20.0f;
				}
				else
				{
					line_marking_angle = marking_angle - 20.0f;	
				}
				
				pid_set_target(&angle_pid, line_marking_angle); /*设置角度环目标*/
				walk_straight++; /*标记完成了一次走直线*/
				in_place_reach_num = 0; /*到位达标次数归零*/
				timeout_event_load(&mission_3_fsm_que, 300, STEERING_IN_PLACE_CHECK); /*挂载第一次到位检查超时事件*/
				me->state = M3_LINE_INSPEC_PRETURN; /*切换至巡线预转向状态*/	
				adjust_counter = 0; /*裁决归零*/
			}
			break;
		}	

		case GRAY_SCALE_WHITE:
		{
			adjust_counter = 0; /*裁决归零*/
		}
	}
}

/*巡线状态事件处理*/
static void line_inspection_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case GRAY_SCALE_CENTER:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_RIGHT_1:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED + OFFSET_1_SPEED_INC);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_RIGHT_2:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED + OFFSET_2_SPEED_INC);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_RIGHT_3:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED + OFFSET_3_SPEED_INC);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_RIGHT_4:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED + OFFSET_4_SPEED_INC);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_RIGHT_5:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED + OFFSET_5_SPEED_INC);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_LEFT_1:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED + OFFSET_1_SPEED_INC); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_LEFT_2:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED + OFFSET_2_SPEED_INC); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_LEFT_3:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED + OFFSET_3_SPEED_INC); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_LEFT_4:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED + OFFSET_4_SPEED_INC); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_LEFT_5:
		{
			pid_set_target(&r_speed_pid, CENTER_SPEED + OFFSET_5_SPEED_INC); /*设置目标速度*/
			pid_set_target(&l_speed_pid, CENTER_SPEED);
			adjust_counter = 0; /*裁决归零*/
			break;
		}
		
		case GRAY_SCALE_WHITE:
		{
			adjust_counter++;
			
			if(adjust_counter == M3_ADJUST_THRE)
			{
				gray_scale_white_handler(me, e);
				adjust_counter = 0;				
			}
			break;
		}
	}
}

/*方向调整状态事件处理*/
static void direction_adjust_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case STEERING_IN_PLACE_CHECK:
		{
			float present_angle, angle_diff;
			imu_get_data(&car_euler_angle, false); /*读取欧拉角*/
			present_angle = car_euler_angle.yaw; /*获取当前偏航角*/
			angle_diff = fabs(imu_math_cal_euler_error(straight_marking_angle, present_angle)); /*计算角度误差*/
			
			if(angle_diff < 1.0)
			{
				in_place_reach_num++;
			}
			else
			{
				in_place_reach_num = 0;
			}
			
			if(in_place_reach_num == M3_ANGLE_IN_PLACE_THRE) /*达到到位标准*/
			{
				pid_output_offset_disable(&angle_pid); /*取消角度环的输出偏移*/
				pid_set_target(&r_speed_pid, 40.0f); /*设置目标速度*/
				pid_set_target(&l_speed_pid, 40.0f);
				gray_twostate_scan_tim_start(&mission_3_fsm_que); /*开启双态灰度扫描*/
				me->state = M3_WALK_STRAIGHT_LINE; /*切换至走直线状态*/				
			}
			else
			{
				timeout_event_load(&mission_3_fsm_que, 300, STEERING_IN_PLACE_CHECK); /*继续挂载到位检查*/
			}
			break;
		}
	}
}

/*巡线预转向事件处理*/
static void line_inspec_preturn(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case STEERING_IN_PLACE_CHECK:
		{
			float present_angle, angle_diff;
			imu_get_data(&car_euler_angle, false); /*读取欧拉角*/
			present_angle = car_euler_angle.yaw; /*获取当前偏航角*/
			angle_diff = fabs(imu_math_cal_euler_error(line_marking_angle, present_angle)); /*计算角度误差*/
			
			if(angle_diff < 1.0)
			{
				in_place_reach_num++;
			}
			else
			{
				in_place_reach_num = 0;
			}
			
			if(in_place_reach_num == M3_ANGLE_IN_PLACE_THRE) /*达到到位标准*/
			{
				
				pid_set_target(&r_speed_pid, 0.0f); /*设置目标速度*/
				pid_set_target(&l_speed_pid, 0.0f);
				pid_disable(&angle_pid); /*失能角度环*/			
				gray_scan_tim_stop(); /*关闭灰度扫描*/
				gray_multi_scan_tim_start(&mission_3_fsm_que); /*开启多态灰度扫描*/
				me->state = M3_LINE_INSPECTION; /*切换至巡线状态*/
			}
			else
			{
				timeout_event_load(&mission_3_fsm_que, 300, STEERING_IN_PLACE_CHECK); /*继续挂载到位检查*/
			}
			break;
		}
	}
}

static void gray_scale_white_handler(state_machine_t* me, param_event_t* e)
{
	/*如果不是最后一次巡线*/
	if(line_inspection != M3_LAST_LINE_INSPECTION)
	{				
		led_set();
		beep_set();
		sound_light_tim_start(); /*执行声光提示*/				
		gray_scan_tim_stop(); /*关闭灰度扫描*/
		pid_set_target(&r_speed_pid, 0.0f); /*设置目标速度*/
		pid_set_target(&l_speed_pid, 0.0f);									
		
		if(IS_ODD(line_inspection)) /*如果是奇数次巡线*/
		{
			straight_marking_angle = marking_angle - 120.75f;
		}
		else
		{
			straight_marking_angle = marking_angle;
		}
		
		pid_set_target(&angle_pid, straight_marking_angle); /*设置角度环目标*/
		pid_enable(&angle_pid); /*使能角度环*/
		line_inspection++; /*标记完成了一次巡线*/
		in_place_reach_num = 0; /*到位达标次数归零*/
		timeout_event_load(&mission_3_fsm_que, 300, STEERING_IN_PLACE_CHECK); /*挂载第一次到位检查超时事件*/
		me->state = M3_DIRECTION_ADJUST; /*切换至方向调整状态*/						
	}
	else
	{
		led_set();
		beep_set();
		sound_light_tim_start(); /*执行声光提示*/
		pid_set_target(&r_speed_pid, 0.0f); /*设置目标速度*/
		pid_set_target(&l_speed_pid, 0.0f);				
		gray_scan_tim_stop(); /*关闭灰度扫描*/
		OLED_ShowString(24, 16, "end of run", OLED_8X16);
		me->state = MISSION_3_IDLE; /*切换至空闲状态*/
	}
}
